
#include <AFMotor.h>
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
AF_DCMotor motor[4]={motor1,motor2,motor3,motor4};
#define MOTORNUM  4
//speed 小于120时，启动不了
void forward_drive(int speed)
{
 
 for(char i=0;i<MOTORNUM;i++)
  {
     motor[i].run(FORWARD);
     motor[i].setSpeed(speed); 
   // motor[i].run(RELEASE);

  }
}
void backward_drive(int speed)
{
for(char i=0;i<MOTORNUM;i++)
  {
     motor[i].run(BACKWARD);
      motor[i].setSpeed(speed); 
   // motor[i].run(RELEASE);

  }

}
void rotate(int rot)
{
   char dir=0;
   char dir_1=0;
  if(rot>0)
  {
    dir=BACKWARD;
    dir_1=FORWARD;


  }
  else
  {
    dir=FORWARD;
    dir_1=BACKWARD;

  }
 
  for(char i=0;i<MOTORNUM;i++)
  {
      if(i==0||i==2)
      {
        motor[i].run(dir);
        motor[i].setSpeed(rot); 
      }
      if(i==1||i==3)
      {
        motor[i].run(dir_1);
        motor[i].setSpeed(rot); 
      }
   // motor[i].run(RELEASE);
  }

  
}
void stop()
{
  for(char i=0;i<MOTORNUM;i++)
  {
    motor[i].setSpeed(0); 
    motor[i].run(RELEASE);

  }

}
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Motor test!");

  // turn on motor
 for(char i=0;i<MOTORNUM;i++)
  {
    motor[i].setSpeed(200); 
    motor[i].run(RELEASE);

  }

 
 
}

void loop() {
  // put your main code here, to run repeatedly:
//前进
    forward_drive(120);
    delay(100);
    forward_drive(80);
    delay(2000);


//停止
    stop();



    delay(1000);

    //旋转
    rotate(200);
    delay(100);
    rotate(140);
    delay(1100);
    stop();
    //前进
    delay(1000);
    forward_drive(120);
    delay(100);
    forward_drive(80);
    delay(2000);
    stop();
    
    while(1)
    {
     

    }

  //step2. back 1m

}
